//
// Created by LiuDongPeng on 2023/11/29.
//

#include "as5047p.h"
#include <math.h>
#include <stdbool.h>

#include "main.h"
#include "spi.h"



/**
 * @brief Check data frame
 * @param dataFrame
 * @return
 */
static bool as5047p_check_data_frame(uint16_t dataFrame)
{
    /**
     * [15]     [14]    [13:0]
     * PARD     EF      DATA
     */

    if (dataFrame & 0x4000)
        return false;
    else
        return true;
}




/**
 * @brief as5047p abs encoder init
 * @param[out]  dev
 * @param[in]   spi_send_recv_handle
 * @param[in]   spi_cs_set_handle
 * @return
 */
int as5047p_init(as5047p_t* dev, spi_send_recv_callback_t spi_send_recv_handle, spi_cs_set_callback_t spi_cs_set_handle)
{
    if (dev == NULL)
        return -1;

    if (spi_send_recv_handle == NULL)
        return -2;

    dev->resolution = 0x3fff;

    dev->spi_send_recv_callback = spi_send_recv_handle;
    dev->spi_cs_set_callback = spi_cs_set_handle;
    dev->cs_valid_level = 0;

    return as5047p_get_raw_data(dev, NULL);
}

/**
 * @brief Get raw data
 * @param[in]   dev
 * @param[out]  raw
 * @return
 */
int as5047p_get_raw_data(as5047p_t* dev, float* raw)
{
    int ret = 0;

    if (dev == NULL)
        return -1;

    /**
     * [15]     [14]    [13:0]
     * PARC     R/W     ADDR
     *
     * addr(0-13bit): 0x3fff
     * read/write(14bit): 1/0   0x3fff | 0x4000
     * parc(15bit): 1/0
     */
//    uint16_t txBuf = AS5047P_ADDR_ANGLECOM | 0x4000 | 0x8000; // addr:  -> 0011 fff -> 0111 fff -> 1111 fff
    uint16_t txBuf = 0xffff;
    uint16_t rxBuf;
    uint16_t nopBuf = AS5047P_ADDR_NOP | 0x4000 | 0x8000;
    uint16_t errBuf = AS5047P_ADDR_ERRFL | 0x4000;

    /* reset cs pin, start transform */
    dev->spi_cs_set_callback(dev->cs_valid_level);

//    ret = HAL_SPI_Transmit(&hspi1, (uint8_t*)&errBuf, 1, 1000);
//    ret = HAL_SPI_TransmitReceive(&hspi1, (uint8_t*)&txBuf, (uint8_t*)&rxBuf, 1, 1000);
//    ret = HAL_SPI_Receive(&hspi1, (uint8_t*)&rxBuf, 1, 1000);

//    dev->spi_send_recv_callback((uint8_t*)&errBuf, (uint8_t*)&rxBuf, 1, 10);
    ret = dev->spi_send_recv_callback((uint8_t*)&txBuf, (uint8_t*)&rxBuf, 1, 1000);
//    ret = dev->spi_send_recv_callback((uint8_t*)&nopBuf, (uint8_t*)&rxBuf, 1, 10);

    /* set cs pin, stop transform */
    dev->spi_cs_set_callback(!dev->cs_valid_level);

    /* Check read data frame */
    if (!as5047p_check_data_frame(rxBuf))
        ret = -2;

    /* Get 14bit data */
    if (raw != NULL)
    {
        rxBuf &= 0x3fff;
        *raw = (float) rxBuf;
    }

    return ret;
}


/**
 * @brief Get angle
 * @param[in]   dev
 * @param[out]  angle
 * @param[out]  rad
 * @return
 */
int as5047p_get_angle(as5047p_t* dev, float* angle, float* rad)
{
    int ret = 0;

    if (!dev || !angle || !rad)
        return -1;

    float raw;
    ret = as5047p_get_raw_data(dev, &raw);

    *angle = raw / dev->resolution * 360.0f;
    *rad = raw / dev->resolution * M_TWOPI;

    return ret;
}
